#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>


#include<cv_bridge/cv_bridge.h>
// #include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <image_transport/image_transport.h>


#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>
#include <map>
#include <pcl/filters/passthrough.h>
#include <chrono>
#include <thread>
#include"depth_rgb_handle/rgb_depth_img.hpp"
#include"depth_rgb_handle/calib.hpp"
#include"depth_rgb_handle/rgb_lidar.hpp"
#include"depth_rgb_handle/pc_handle.hpp"
#include"depth_rgb_handle/utils.hpp"
#include <X11/Xlib.h>

using namespace std;
using PointType = pcl::PointXYZI;


//rgb depth img handle
RgbDepthHandle rgb_depth_handle_;

//rgb lidar handle
RgbLidarHandle rgb_lidar_handle_;

//pc handle
PCHandle pc_handle_;

//gui 
cv::String window_name_rs_ = "realsense";
cv::String window_name_ld_ = "lidar";

string kb_inf_;

//object pointcloud
pcl::PointCloud<PointType>::Ptr object_pc_;

//display
ros::Publisher  cloud_pub_, bbox_pub_, pc_plane_pub_, bbox_plane_pub_;

void capture_event_rs(int event, int x, int y, int flags, void *params){
    if (event == cv::EVENT_LBUTTONDOWN) {
        rgb_depth_handle_.record_point(x, y);
    }
    if (event == cv::EVENT_RBUTTONDOWN) {
        ROS_INFO("rs_handle callback ok");
        pc_handle_.get_object_pc(rgb_depth_handle_.get_object_points_rs());
        ROS_INFO("rs_handle ok");
        pc_handle_.get_bbox_save();
        ROS_INFO("rs pc_handle_.get_bbox_save ok");
        // pc_handle_.get_object_pc(object_pc_);
        // pc_handle_.get_bbox();
        // cv::imshow("mask",rgb_depth_handle_.mask_);
        // cv::waitKey(1);
        cout<<"get object pointcloud ok!!!!!!"<<endl;
    }
}

void capture_event_ld(int event, int x, int y, int flags, void *params){
    if (event == cv::EVENT_LBUTTONDOWN) {
        rgb_lidar_handle_.record_point(x, y);
    }
    if (event == cv::EVENT_RBUTTONDOWN) {
        pc_handle_.get_object_pc(rgb_lidar_handle_.get_object_points_ld());
        ROS_INFO("rgb_lidar_handle ok");
        pc_handle_.get_bbox_save();
        ROS_INFO("pc_handle_.get_bbox_save ok");
        // cv::imshow("mask",rgb_depth_handle_.mask_);
        // cv::waitKey(1);
        cout<<"get object pointcloud ok!!!!!!"<<endl;
    }
}

void img_CB(const sensor_msgs::ImageConstPtr& rgb_img_msg, const sensor_msgs::ImageConstPtr& depth_img_msg){
    float T_rs2base[12] = {-0.0066,    0.9999,    0.0097, -0.1462, 
                                            0.0445,    0.0100,   -0.9990, 0.0111,
                                               -0.9990,   -0.0061,   -0.0445, -0.0810}; //to do get tf from hand

    rgb_depth_handle_.get_img_extrinsics(cv_bridge::toCvCopy(rgb_img_msg, sensor_msgs::image_encodings::BGR8)->image, 
                                                                                        cv_bridge::toCvCopy(depth_img_msg, sensor_msgs::image_encodings::TYPE_16UC1)->image, T_rs2base,
                                                                                        rgb_img_msg->header.stamp.toSec());
    cv::namedWindow(window_name_rs_);
    cv::moveWindow(window_name_rs_, 10,10);
	cv::setMouseCallback(window_name_rs_, capture_event_rs);
    rgb_depth_handle_.draw_contours(kb_inf_);
    cv::imshow(window_name_rs_, rgb_depth_handle_.rgb_img_show_);
    cv::waitKey(1);
    // ros::Duration(0.015).sleep();

}

void pc_rgb_CB(const sensor_msgs::PointCloud2ConstPtr& pc_msg, const sensor_msgs::ImageConstPtr& rgb_img_msg){
    // kb_inf_ = get_keyboard();
    pcl::PointCloud<PointType>::Ptr pc_temp(new pcl::PointCloud<PointType>);
    pcl::fromROSMsg(*pc_msg, *pc_temp);
    rgb_lidar_handle_.get_pc_img(pc_temp, cv_bridge::toCvCopy(rgb_img_msg, sensor_msgs::image_encodings::BGR8)->image, pc_msg->header.stamp.toSec());
    cv::namedWindow(window_name_ld_);
    cv::moveWindow(window_name_ld_, 10,550);
	cv::setMouseCallback(window_name_ld_, capture_event_ld);
    rgb_lidar_handle_.draw_contours( kb_inf_);
    cv::imshow(window_name_ld_, rgb_lidar_handle_.lidar_img_show_);
    cv::waitKey(1);
}

void display_th(){
    while(true){
        // cout<<"pub"<<endl;
        if(rgb_lidar_handle_.is_get_object_pc_){
            display_pc(pc_handle_.local_object_pc_.p_object_pc, cloud_pub_);
            // vector<DataRet> ret;
            // ret.push_back(pc_handle_.local_obj_inf_);
            // display_bbox(ret);
            display_bbox(pc_handle_.global_obj_inf_, bbox_pub_);
            display_pc_for_measure(pc_handle_.global_obj_inf_, pc_plane_pub_, bbox_plane_pub_);
            // cout<<"object center:  "<< pc_handle_.object_center_.x << "  "<<pc_handle_.object_center_.y << "  "<<pc_handle_.object_center_.z << "  "<<endl;
            // cout<<"object center:  "<< rgb_lidar_handle_.object_center_.x << "  "<<rgb_lidar_handle_.object_center_.y << "  "<<rgb_lidar_handle_.object_center_.z << "  "<<endl;
        }
        if(rgb_depth_handle_.is_get_object_pc_){
            display_pc(pc_handle_.local_object_pc_.p_object_pc, cloud_pub_);
            // vector<cv::Mat> ret;
            // ret.push_back(pc_handle_.local_object_bbox_);
            display_bbox(pc_handle_.global_obj_inf_, bbox_pub_);
            // display_pc_for_measure(pc_handle_.global_obj_inf_);
            display_pc_for_measure(pc_handle_.global_obj_inf_, pc_plane_pub_, bbox_plane_pub_);
            // cout<<"object center:  "<< pc_handle_.object_center_.x << "  "<<pc_handle_.object_center_.y << "  "<<pc_handle_.object_center_.z << "  "<<endl;
            // cout<<"object center:  "<< rgb_depth_handle_.object_center_.x << "  "<<rgb_depth_handle_.object_center_.y << "  "<<rgb_depth_handle_.object_center_.z << "  "<<endl;
        }
        ros::Duration(0.2).sleep();
    }
}

void get_key_th(){
    while(true){
        while(kb_inf_.empty()){
            kb_inf_ = get_keyboard();
        }
        // cout<<"key th"<<endl;
    }
}

void get_tf_th(){
    while(true){
        // cout<<"get tf key"<<endl;
        pc_handle_.get_tf(get_tf("/map", "base_link"));
        // ros::Duration(0.1).sleep();
    }
}

int main(int argc, char* argv[])
{
    ros::init(argc, argv, "det_handle_img");
    ros::NodeHandle nh;
    message_filters::Subscriber<sensor_msgs::Image> image_front_sub(nh, "/camera/color/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::Image> depth_image_sub(nh, "/camera/aligned_depth_to_color/image_raw", 5);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pc_sub(nh, "velodyne_points", 5);
    message_filters::Subscriber<sensor_msgs::Image> rgb_usb_sub(nh, "/camera/color/image_raw", 5);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(20), image_front_sub, depth_image_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.05));
    sync.registerCallback(boost::bind(&img_CB, _1, _2));

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::PointCloud2, sensor_msgs::Image> MySyncPolicy2;
    message_filters::Synchronizer<MySyncPolicy2> sync2(MySyncPolicy2(20),  pc_sub, rgb_usb_sub);
    sync2.setMaxIntervalDuration(ros::Duration(0.05));
    sync2.registerCallback(boost::bind(&pc_rgb_CB, _1, _2));

    // image_transport::ImageTransport it(nh);
    // image_pub_ = it.advertise("result_img", 1);

    cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("object_pc",1);
    bbox_pub_=nh.advertise<visualization_msgs::MarkerArray>("bbox", 1);
    pc_plane_pub_ = nh.advertise<sensor_msgs::PointCloud2>("object_pc_for_measure",1);
    bbox_plane_pub_=nh.advertise<visualization_msgs::MarkerArray>("bbox_for_measure", 1);

    XInitThreads();
    // ros::AsyncSpinner spinner(boost::thread::hardware_concurrency());
    // spinner.start();

    thread th1(display_th);
    thread th2(get_tf_th);
    thread th3(get_key_th);
    th1.detach();
    th2.detach();
    th3.detach();

    // while(true){

    //     ros::spinOnce();
    // }
    ros::spin();

    return 0;
}
